// Tom's amazing moving wall program
// rev 0.2
// 09/20/2012

/////////////////////////////////////////////////////////////
// Environmental includes
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#include "SoftwareSerial.h"
SoftwareSerial port(12,13);
#else // Arduino 0022 - use modified NewSoftSerial
#include "WProgram.h"
#include "NewSoftSerial.h"
NewSoftSerial port(12,13);
#endif

#include "EasyVR.h"


////////////////////////////////////////////////////////////
// Defines
#define MAX_SPEED 255
#define REV_MAX_SPEED 180 // constant deffinition to support Maximum reverse speed //tt//
#define ON  HIGH
#define OFF LOW
#define HSPin 9
#define FORWARD 11  // forward Pin
#define REVERSE 10  // reverse Pin

//////////////////////////////////////////////////////////
// Global Vars
EasyVR easyvr(port);
EasyVRBridge bridge;




///////////////////////////////////////////////////////////////
// Required Setup function
//////////////////////////////////////////////////////////////
void setup()
{
  pinMode(HSPin,OUTPUT);
  pinMode(FORWARD, OUTPUT);
  digitalWrite (FORWARD, LOW);
  pinMode(REVERSE, OUTPUT);


  // bridge mode?
  if (bridge.check())
  {
    cli();
    bridge.loop(0, 1, 12, 13);
  }

  Serial.begin(9600);
  port.begin(9600);

  if (!easyvr.detect())
  {
    Serial.println("EasyVR not detected!");
    for (;;);
  }

  easyvr.setPinOutput(EasyVR::IO1, LOW);
  Serial.println("EasyVR detected!");
  easyvr.setTimeout(5);
  easyvr.setLanguage(EasyVR::ENGLISH);

  int16_t count = 0;
  uint32_t mask = 0;
  int8_t group = 0;

  if (easyvr.getGroupMask(mask)) // get trained user names and passwords
  {
    uint32_t msk = mask;
    for (group = 0; group <= EasyVR::PASSWORD; ++group, msk >>= 1)
    {
      if (!(msk & 1)) continue;
      if (group == EasyVR::TRIGGER)
        Serial.print("Trigger: ");
      else if (group == EasyVR::PASSWORD)
        Serial.print("Password: ");
      else
      {
        Serial.print("Group ");
        Serial.print(group);
        Serial.print(": ");
      }
      count = easyvr.getCommandCount(group);
      Serial.println(count);
      for (int8_t idx = 0; idx < count; ++idx)
      {
        char name[32];
        uint8_t train = 0;
        if (easyvr.dumpCommand(group, idx, name, train))
        {
          Serial.print(idx);
          Serial.print(" = ");
          Serial.print(name);
          Serial.print(", Trained ");
          Serial.print(train, DEC);
          if (!easyvr.isConflict())
            Serial.println(" times, OK");
          else
          {
            int8_t confl = easyvr.getWord();
            if (confl >= 0)
              Serial.print(" times, Similar to Word ");
            else
            {
              confl = easyvr.getCommand();
              Serial.print(" times, Similar to Command ");
            }
            Serial.println(confl);
          }
        }
      }
    }
  }
  easyvr.setLevel(EasyVR::HARD);  //I am finding it difficult to consistently get accurate regognition, changed from HARDER to NORMAL //tt//
}

//////////////////////////////////////////////////////////////////////////////
// Main continuous loop
//////////////////////////////////////////////////////////////////////////////
void loop()
{
  // wait until we get a Voice command
  int idx_cmd = getVRCommand();
  if (idx_cmd >= 0)
  {
    displayCommand(idx_cmd);
    // Do Fun Stuff here -----------------------------------------------
    executeCommand(idx_cmd);
    // End of Fun Stuff ------------------------------------------------
  }
  else // errors or timeout
  {
    displayTimeoutAndErrors();
  }
}


////////////////////////////////////////////////////////////////////////////
// Start support functions
////////////////////////////////////////////////////////////////////////////
// Function to execute the choosen command, this maps the cmd index to a function to run for that index
void executeCommand(int cmd)
{
 switch (cmd) {
  case 0:
    Serial.println("Running Cmd 0");
    break;
  case 1:    //
    Serial.println("Running Cmd 1: rampMotorUpAndDownForwardFullWithFan()");
    rampMotorUpAndDownForwardFullWithFan();
    break;
  case 2:
    Serial.println("Running Cmd 2:rampMotorUpAndDownReverseFullWithFan()"); //Attempt to issue a reverse command on G1I2 recognition //tt//
    rampMotorUpAndDownReverseFullWithFan();  //Attempt to issue a reverse command on G1I2 recognition //tt//
    break;
  case 3:
    Serial.println("Running Cmd 1:rampMotorUpAndDownForwardHalfWayWithFan()");
    rampMotorUpAndDownForwardHalfWayWithFan();
    break;
  case 4:
    Serial.println("Running Cmd 2:rampMotorUpAndDownReverseHalfWayWithFan()"); //Attempt to issue a reverse command on G1I2 recognition //tt//
    rampMotorUpAndDownReverseHalfWayWithFan();
    break;
  case 5:
    Serial.println("Running Cmd 5");
    break;
  case 6:
    Serial.println("Running Cmd 6");
    break;
  case 7:
    Serial.println("Running Cmd 7");
    break;
  case 8:
    Serial.println("Running Cmd 8");
    break;
  case 9:
    Serial.println("Running Cmd 9");
    break;
  case 10:
    Serial.println("Running Cmd 10");
    break;
  // add more cases as needed
  default:
    Serial.print("Unknown cmd value: ");
    Serial.println(cmd);
 }
}


////////////////////////////////////////////////////////////////////////////
void turnHeatSinkFan(int onoff)
{
    digitalWrite(HSPin,onoff);
}


//////////////////////////////////////////////////////////////////////////
void rampMotorUpAndDownForwardFullWithFan()
{
    turnHeatSinkFan(ON);
     // Ramp up motor to high speed and then ramp back down
    int currentSpeed = MotorRampUp(FORWARD);
    delay(7280); //Hold at full speed 
    MotorRampDown(currentSpeed,FORWARD);
    delay(4000);  //Hold the cooling fan on after the drive motor stops
    turnHeatSinkFan(OFF);
}

void rampMotorUpAndDownReverseFullWithFan() // A copy of above function to support rampUpandDown in reverse 
{
    turnHeatSinkFan(ON);
     // Ramp up motor to high speed and then ramp back down
    int currentSpeed = MotorRampUp(REVERSE);  
    delay(8430); //Hold at full speed
    MotorRampDown(currentSpeed, REVERSE); 
    delay(3000);  //Hold the cooling fan on after the drive motor stops
    turnHeatSinkFan(OFF);

}

void rampMotorUpAndDownForwardHalfWayWithFan()
{
    turnHeatSinkFan(ON);
     // Ramp up motor to high speed and then ramp back down
    int currentSpeed = MotorRampUp(FORWARD);
    delay(400); //Hold at full speed before ramping down
    MotorRampDown(currentSpeed, FORWARD);
    delay(4000);  //Hold the cooling fan on after the drive motor stops
    turnHeatSinkFan(OFF);
}

void rampMotorUpAndDownReverseHalfWayWithFan() // A copy of above function to support rampUpandDown in reverse //tt//
{
    turnHeatSinkFan(ON);
     // Ramp up motor to high speed and then ramp back down
    int currentSpeed = MotorRampUp(REVERSE);  //Reverese for Motor ramp Up //tt//
    delay(1150); //Hold at full speed for 2 seconds before ramping down//tt//
    MotorRampDown(currentSpeed,REVERSE); //Reverse for Motor ramp Down //tt//
    delay(3000);  //Hold the cooling fan on for 3 seconds after the drive motor stops //tt//
    turnHeatSinkFan(OFF);

}
///////////////////////////////////////////////////////////////////////////
// this function will wait until it gets a Voice command
// turns on the LED while listening then back off when got a command
int getVRCommand()
{
  easyvr.setPinOutput(EasyVR::IO1, HIGH); // LED on (listening)

  Serial.println("Say a Command in Group 1");
  easyvr.recognizeCommand(1); // recognise command in group 1

  while (!easyvr.hasFinished()); // wait for command

  easyvr.setPinOutput(EasyVR::IO1, LOW); // LED off
  return easyvr.getCommand(); // get recognised command
}


/////////////////////////////////////////////////////////////////////////
void displayCommand(int idx_cmd)
{
    Serial.print("Command Name: ");
    char name[32];
    uint8_t train = 0;

    if (easyvr.dumpCommand(1, idx_cmd, name, train))
      Serial.println(name);
    else
      Serial.println("Not recognized");
}


//////////////////////////////////////////////////////////////////////////
void displayTimeoutAndErrors()
{
    if (easyvr.isTimeout())
      Serial.println("Timed out, try again...");

    int16_t err = easyvr.getError();
    if (err >= 0)
    {
      Serial.print("Error ");
      Serial.println(err, HEX);
    }
}


/////////////////////////////////////////////////////////////////////////
// Start from 50 and increment speed by 25 every 250 millis until we reach max speed
// returns last the speed reached
int MotorRampUp(int direction)
{
  int max = REV_MAX_SPEED;
  allOff();
  if (direction == FORWARD) max = MAX_SPEED;
  
  int speed;
  for (speed = 50; speed < max; speed=speed+25)
  {
    analogWrite(direction, speed);
    delay(250);
  }
  return speed;
}


////////////////////////////////////////////////////////////////////////
void MotorRampDown(int currentSpeed, int direction)
{
  for (int speed = currentSpeed-50; speed > 0; speed=speed-50) // First time through motor stops hard
  {
    analogWrite(direction, speed);
    delay(650); 
  }
  // insure go to 0
  analogWrite(direction, 0);
  allOff();
}


////////////////////////////////////////////////////////////////////////
void allOff()
{
  digitalWrite(FORWARD, LOW);
  digitalWrite(REVERSE, LOW);
  delay(1);
}
